#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Sensor, S2,     sonar2,         sensorSONAR)
#pragma config(Sensor, S3,     HTSPB,                sensorI2CCustom9V)
#pragma config(Sensor, S4,     HTGYRO,         sensorAnalogInactive)
#pragma config(Motor,  mtr_S1_C1_1,     left,          tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     hdfh,          tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     right,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    servo1,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#define sonar4 0

#include "geary-nomux-stuff.c"
#include "geary-nomux-gyro-stuff.c"

//////////////////////////////////////////////////////////////////////////////
//
//      MAIN
//
/////////////////////////////////////////////////////////////////////////////


task
main ()
{
	DriveByButtonSpeed = 30; // override the default slow motor speed

  float startHeading = currHeading;
  float flipHeading = startHeading + 180;
  const int onewayDistance = 2000;

  StartTask(getHeading);
  wait1Msec(300);
  StartTask(AvoidWalls);
  wait1Msec(300);

  InitProtoboardLights();
  wait1Msec(1000);

  nMotorEncoder[right] = 0;
  while (nMotorEncoder[right] > -onewayDistance) {
    GoStraightByGyro(startHeading);
    nxtDisplayTextLine(6, "enc: %d", nMotorEncoder[right]);
	}
  StopMotors();

  StopTask(getHeading);
  wait1Msec(300);
  StartTask(getHeading);
  wait1Msec(300);

	Turn180(10);

  nMotorEncoder[right] = 0;
  while (nMotorEncoder[right] > -onewayDistance/2) {
    GoStraightByGyro(flipHeading);
    nxtDisplayTextLine(6, "enc: %d", nMotorEncoder[right]);
	}
  StopMotors();

  StopAllTasks ();
}
